#ifndef CATACAMERA_H
#define CATACAMERA_H

#include <opencv2/core/core.hpp>
#include <string>

#include "ceres/rotation.h"
#include "Camera.h"

namespace camodocal
{

    /**
     * C. Mei, and P. Rives, Single View Point Omnidirectional Camera Calibration
     * from Planar Grids, ICRA 2007
     */

    class CataCamera : public Camera
    {
    public:
        class Parameters : public Camera::Parameters
        {
        public:
            Parameters();
            Parameters(const std::string &cameraName,
                       int w, int h,
                       double xi,
                       double k1, double k2, double p1, double p2,
                       double gamma1, double gamma2, double u0, double v0);

            double &xi(void);
            double &k1(void);
            double &k2(void);
            double &p1(void);
            double &p2(void);
            double &gamma1(void);
            double &gamma2(void);
            double &u0(void);
            double &v0(void);

            double xi(void) const;
            double k1(void) const;
            double k2(void) const;
            double p1(void) const;
            double p2(void) const;
            double gamma1(void) const;
            double gamma2(void) const;
            double u0(void) const;
            double v0(void) const;

            bool readFromYamlFile(const std::string &filename);
            void writeToYamlFile(const std::string &filename) const;

            Parameters &operator=(const Parameters &other);
            friend std::ostream &operator<<(std::ostream &out, const Parameters &params);

        private:
            double m_xi;
            double m_k1;
            double m_k2;
            double m_p1;
            double m_p2;
            double m_gamma1;
            double m_gamma2;
            double m_u0;
            double m_v0;
        };

        CataCamera();

        /**
         * \brief Constructor from the projection model parameters
         */
        CataCamera(const std::string &cameraName,
                   int imageWidth, int imageHeight,
                   double xi, double k1, double k2, double p1, double p2,
                   double gamma1, double gamma2, double u0, double v0);
        /**
         * \brief Constructor from the projection model parameters
         */
        CataCamera(const Parameters &params);

        Camera::ModelType modelType(void) const;
        const std::string &cameraName(void) const;
        int imageWidth(void) const;
        int imageHeight(void) const;

        void estimateIntrinsics(const cv::Size &boardSize,
                                const std::vector<std::vector<cv::Point3f>> &objectPoints,
                                const std::vector<std::vector<cv::Point2f>> &imagePoints);

        // Lift points from the image plane to the sphere
        void liftSphere(const Eigen::Vector2d &p, Eigen::Vector3d &P) const;
        //%output P

        // Lift points from the image plane to the projective space
        void liftProjective(const Eigen::Vector2d &p, Eigen::Vector3d &P) const;
        //%output P

        // Projects 3D points to the image plane (Pi function)
        void spaceToPlane(const Eigen::Vector3d &P, Eigen::Vector2d &p) const;
        //%output p

        // Projects 3D points to the image plane (Pi function)
        // and calculates jacobian
        void spaceToPlane(const Eigen::Vector3d &P, Eigen::Vector2d &p,
                          Eigen::Matrix<double, 2, 3> &J) const;
        //%output p
        //%output J

        void undistToPlane(const Eigen::Vector2d &p_u, Eigen::Vector2d &p) const;
        //%output p

        template <typename T>
        static void spaceToPlane(const T *const params,
                                 const T *const q, const T *const t,
                                 const Eigen::Matrix<T, 3, 1> &P,
                                 Eigen::Matrix<T, 2, 1> &p);

        void distortion(const Eigen::Vector2d &p_u, Eigen::Vector2d &d_u) const;
        void distortion(const Eigen::Vector2d &p_u, Eigen::Vector2d &d_u,
                        Eigen::Matrix2d &J) const;

        void initUndistortMap(cv::Mat &map1, cv::Mat &map2, double fScale = 1.0) const;
        cv::Mat initUndistortRectifyMap(cv::Mat &map1, cv::Mat &map2,
                                        float fx = -1.0f, float fy = -1.0f,
                                        cv::Size imageSize = cv::Size(0, 0),
                                        float cx = -1.0f, float cy = -1.0f,
                                        cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;

        int parameterCount(void) const;

        const Parameters &getParameters(void) const;
        void setParameters(const Parameters &parameters);

        void readParameters(const std::vector<double> &parameterVec);
        void writeParameters(std::vector<double> &parameterVec) const;

        void writeParametersToYamlFile(const std::string &filename) const;

        std::string parametersToString(void) const;

    private:
        Parameters mParameters;

        double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;
        bool m_noDistortion;
    };

    typedef boost::shared_ptr<CataCamera> CataCameraPtr;
    typedef boost::shared_ptr<const CataCamera> CataCameraConstPtr;

    template <typename T>
    void
    CataCamera::spaceToPlane(const T *const params,
                             const T *const q, const T *const t,
                             const Eigen::Matrix<T, 3, 1> &P,
                             Eigen::Matrix<T, 2, 1> &p)
    {
        T P_w[3];
        P_w[0] = T(P(0));
        P_w[1] = T(P(1));
        P_w[2] = T(P(2));

        // Convert quaternion from Eigen convention (x, y, z, w)
        // to Ceres convention (w, x, y, z)
        T q_ceres[4] = {q[3], q[0], q[1], q[2]};

        T P_c[3];
        ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);

        P_c[0] += t[0];
        P_c[1] += t[1];
        P_c[2] += t[2];

        // project 3D object point to the image plane
        T xi = params[0];
        T k1 = params[1];
        T k2 = params[2];
        T p1 = params[3];
        T p2 = params[4];
        T gamma1 = params[5];
        T gamma2 = params[6];
        T alpha = T(0); // cameraParams.alpha();
        T u0 = params[7];
        T v0 = params[8];

        // Transform to model plane
        T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]);
        P_c[0] /= len;
        P_c[1] /= len;
        P_c[2] /= len;

        T u = P_c[0] / (P_c[2] + xi);
        T v = P_c[1] / (P_c[2] + xi);

        T rho_sqr = u * u + v * v;
        T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr;
        T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u);
        T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v;

        u = L * u + du;
        v = L * v + dv;
        p(0) = gamma1 * (u + alpha * v) + u0;
        p(1) = gamma2 * v + v0;
    }

}

#endif
